作動関節マニピュレータの逆運動学

Created by Masahiro Furukawa, Aug 18, 2020

#参考URL -> https://qiita.com/tibigame/items/61cecf86fc978628bfee
#参考図書 -> ポールのロボット・マニピュレータ

import numpy as np
import sympy as sym
sym.init_printing()
Pi = sym.S.Pi # 円周率

#sympyの円周率の方を使うことをすすめる（こっちの方が量子化誤差が大きくなる．numpyも同様に大きい）
import math
pi = math.pi

# 角度変数
(J_1,J_2,J_3,J_4,J_5,J_6) = sym.symbols('J_1,J_2,J_3,J_4,J_5,J_6')

# リンクパラメータ
(a_1,a_2,a_3,d_4) = sym.symbols('a_1,a_2,a_3,d_4')

# リンクパラメータ
(j,a,d,alpha) = sym.symbols('j,a,d,alpha')

# T6
(n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z) = sym.symbols('n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z')

#sin.cosの簡易記述用
def S(a):
    return sym.sin(a)

def C(a):
    return sym.cos(a)

#回転・並進行列
def rotx(a):
    return sym.Matrix([[1, 0, 0, 0], [0, C(a), -S(a), 0], [0, S(a), C(a), 0], [0, 0, 0, 1]])

def roty(a):
    return sym.Matrix([[C(a), 0, S(a), 0], [0, 1, 0, 0], [-S(a), 0, C(a), 0], [0, 0, 0, 1]])

def rotz(a):
    return sym.Matrix([[C(a), -S(a), 0, 0], [S(a), C(a), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])

def trans(x, y, z):
    return sym.Matrix([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]])

# DH matrix 
def DH(j, alpha, a, d):
    return rotz(j)*trans(a,0,d)*rotx(alpha)

# inverse DH matrix
def DHi(j, alpha, a, d):
    return rotx(-alpha)*trans(-a,0,-d)*rotz(-j)

# target項を式eqからくくり出すための関数
def obs(eq, target):
    sol = sym.solve(eq, target)
    for i  in range(len(sol)):
        display( sym.Eq(target,  sol[i]))
    print( str(len(sol)) +' equation(s) in total')
    return sol

DH法に基づく座標系間の関係表現

座標系 i	Z_i-1軸回りに角度θ_i	X_i軸周りにねじれ角α_iだけ回転	回転後のX_i-1 (=X_i)に沿って長さa_iだけ並進	Z_i-1に沿って距離d_iだけ並進
1	$J_1$	$\pi/2$	$a_1$	0
2	$J_2+\pi/2$	0	$a_2$	0
3	$J_3-J_2$	$\pi/2$	$a_3$	0
4	$J_4$	$-\pi/2$	0	$d_4$
5	$J_5$	$\pi/2$	0	0
6	$J_6$	0	0	0	$

変換行列 A

easy=False
if(easy):
    A1=sym.trigsimp( DH (J_1,             Pi/2, 0, 0))
    A3=sym.trigsimp( DH (J_3 - J_2,   Pi/2, 0, 0))
    A1i=sym.trigsimp( DHi (J_1,             Pi/2, 0, 0))
    A3i=sym.trigsimp( DHi (J_3 - J_2,   Pi/2, 0, 0))
else:
    A1=sym.trigsimp( DH (J_1,             Pi/2, a_1, 0))
    A3=sym.trigsimp( DH (J_3 - J_2,   Pi/2, a_3, 0))
    # inverse matrix
    A1i=sym.trigsimp( DHi (J_1,             Pi/2, a_1, 0))
    A3i=sym.trigsimp( DHi (J_3 - J_2,   Pi/2, a_3, 0))

A2=sym.trigsimp( DH (J_2+ Pi/2,       0, a_2, 0))
A4=sym.trigsimp( DH (J_4,            -Pi/2,      0, d_4))
A5=sym.trigsimp( DH (J_5,             Pi/2,      0, 0))
A6=sym.trigsimp( DH (J_6,                   0,     0, 0))

# inverse matrix
A2i=sym.trigsimp( DHi (J_2+ Pi/2,       0, a_2, 0))
A4i=sym.trigsimp( DHi (J_4,            -Pi/2,      0, d_4))
A5i=sym.trigsimp( DHi (J_5,             Pi/2,      0, 0))
A6i=sym.trigsimp( DHi (J_6,                   0,     0, 0))

A3

$\displaystyle \left[\begin{matrix}\cos{\left(J_{2} - J_{3} \right)} & 0 & - \sin{\left(J_{2} - J_{3} \right)} & a_{3} \cos{\left(J_{2} - J_{3} \right)}\\- \sin{\left(J_{2} - J_{3} \right)} & 0 & - \cos{\left(J_{2} - J_{3} \right)} & - a_{3} \sin{\left(J_{2} - J_{3} \right)}\\0 & 1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

# 逆行列をかけると単位行列になることの確認
ret = A2i*A2
sym.trigsimp(ret)

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

逆運動学

T6=sym.Matrix([[n_x, o_x, a_x, p_x], [n_y, o_y, a_y, p_y], [n_z, o_z, a_z, p_z], [0, 0, 0, 1]])
T6

$\displaystyle \left[\begin{matrix}n_{x} & o_{x} & a_{x} & p_{x}\\n_{y} & o_{y} & a_{y} & p_{y}\\n_{z} & o_{z} & a_{z} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right]$

# forward kinematics
A56 = sym.trigsimp( A5*A6 )
A456 = sym.trigsimp( A4*A5*A6 )
A3456 = sym.trigsimp( A3*A4*A5*A6 )
A23456 = sym.trigsimp( A2*A3*A4*A5*A6 )
T  = sym.trigsimp( A1*A2*A3*A4*A5*A6 )

T

$\displaystyle \left[\begin{matrix}\left(\left(\sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) \cos{\left(J_{5} \right)} - \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{6} \right)} + \left(\sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) \sin{\left(J_{6} \right)} & - \left(\left(\sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) \cos{\left(J_{5} \right)} - \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{6} \right)} + \left(\sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) \cos{\left(J_{6} \right)} & \left(\sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) \sin{\left(J_{5} \right)} + \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} & \left(a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{1} \right)}\\\left(- \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) \cos{\left(J_{5} \right)} - \sin{\left(J_{1} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{6} \right)} + \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) \sin{\left(J_{6} \right)} & - \left(- \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) \cos{\left(J_{5} \right)} - \sin{\left(J_{1} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{6} \right)} + \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) \cos{\left(J_{6} \right)} & - \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) \sin{\left(J_{5} \right)} + \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} & \left(a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{1} \right)}\\\left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \cos{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} & - \left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \sin{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

順運動学計算用Cソースコードを得る

# Masahiro Furukawa
# Aug, 17, 2020
#
# refernce : https://qiita.com/JmpM/items/4bea4997aaf406cca3b4

# Cソースを得る
for ii in range(4):
    for jj in range(4):
        idx = jj*4+ii
        code = sym.ccode(T[idx],assign_to=('Trans['+str(jj)+']['+str(ii)+']'), standard='C89')
        print(code)
    print()

Trans[0][0] = ((sin(J_1)*sin(J_4) - sin(J_3)*cos(J_1)*cos(J_4))*cos(J_5) - sin(J_5)*cos(J_1)*cos(J_3))*cos(J_6) + (sin(J_1)*cos(J_4) + sin(J_3)*sin(J_4)*cos(J_1))*sin(J_6);
Trans[1][0] = (-(sin(J_1)*sin(J_3)*cos(J_4) + sin(J_4)*cos(J_1))*cos(J_5) - sin(J_1)*sin(J_5)*cos(J_3))*cos(J_6) + (sin(J_1)*sin(J_3)*sin(J_4) - cos(J_1)*cos(J_4))*sin(J_6);
Trans[2][0] = (-sin(J_3)*sin(J_5) + cos(J_3)*cos(J_4)*cos(J_5))*cos(J_6) - sin(J_4)*sin(J_6)*cos(J_3);
Trans[3][0] = 0;
()
Trans[0][1] = -((sin(J_1)*sin(J_4) - sin(J_3)*cos(J_1)*cos(J_4))*cos(J_5) - sin(J_5)*cos(J_1)*cos(J_3))*sin(J_6) + (sin(J_1)*cos(J_4) + sin(J_3)*sin(J_4)*cos(J_1))*cos(J_6);
Trans[1][1] = -(-(sin(J_1)*sin(J_3)*cos(J_4) + sin(J_4)*cos(J_1))*cos(J_5) - sin(J_1)*sin(J_5)*cos(J_3))*sin(J_6) + (sin(J_1)*sin(J_3)*sin(J_4) - cos(J_1)*cos(J_4))*cos(J_6);
Trans[2][1] = -(-sin(J_3)*sin(J_5) + cos(J_3)*cos(J_4)*cos(J_5))*sin(J_6) - sin(J_4)*cos(J_3)*cos(J_6);
Trans[3][1] = 0;
()
Trans[0][2] = (sin(J_1)*sin(J_4) - sin(J_3)*cos(J_1)*cos(J_4))*sin(J_5) + cos(J_1)*cos(J_3)*cos(J_5);
Trans[1][2] = -(sin(J_1)*sin(J_3)*cos(J_4) + sin(J_4)*cos(J_1))*sin(J_5) + sin(J_1)*cos(J_3)*cos(J_5);
Trans[2][2] = sin(J_3)*cos(J_5) + sin(J_5)*cos(J_3)*cos(J_4);
Trans[3][2] = 0;
()
Trans[0][3] = (a_1 - a_2*sin(J_2) - a_3*sin(J_3) + d_4*cos(J_3))*cos(J_1);
Trans[1][3] = (a_1 - a_2*sin(J_2) - a_3*sin(J_3) + d_4*cos(J_3))*sin(J_1);
Trans[2][3] = a_2*cos(J_2) + a_3*cos(J_3) + d_4*sin(J_3);
Trans[3][3] = 1;
()

inverse kinematics

$$ % reference : https://qiita.com/namoshika/items/63db972bfd1030f8264a % 空白は表示に影響しない。コメントは"%"で始める % 下付き文字は"_a"、上付き文字は"^a" % 改行は"\\"を付ける {\boldsymbol{A}_{1}}^{-1} \boldsymbol{T}_6 = \boldsymbol{A}_2 \boldsymbol{A}_3 \boldsymbol{A}_4 \boldsymbol{A}_5 \boldsymbol{A}_6 　　　(3.75)\\ {\boldsymbol{A}_{1}}^{-1} \boldsymbol{T}_6 = ^{1}\boldsymbol{T}_6 　　　(3.76) % 複数文字を1要素とする際は{...}で囲う % 空白は"\quad" $$

T16 = sym.trigsimp(                         A1i*T6 ) #  eq(3.70)
T26 = sym.trigsimp(                   A2i*A1i*T6 ) #  eq(3.71)
T36 = sym.trigsimp(             A3i*A2i*A1i*T6 ) #  eq(3.72)
T46 = sym.trigsimp(       A4i*A3i*A2i*A1i*T6 ) #  eq(3.73)
T56 = sym.trigsimp( A5i*A4i*A3i*A2i*A1i*T6 ) #  eq(3.74)

# Left hand  of (3.76)
A1iT6 = T16
A1iT6

$\displaystyle \left[\begin{matrix}n_{x} \cos{\left(J_{1} \right)} + n_{y} \sin{\left(J_{1} \right)} & o_{x} \cos{\left(J_{1} \right)} + o_{y} \sin{\left(J_{1} \right)} & a_{x} \cos{\left(J_{1} \right)} + a_{y} \sin{\left(J_{1} \right)} & - a_{1} + p_{x} \cos{\left(J_{1} \right)} + p_{y} \sin{\left(J_{1} \right)}\\n_{z} & o_{z} & a_{z} & p_{z}\\n_{x} \sin{\left(J_{1} \right)} - n_{y} \cos{\left(J_{1} \right)} & o_{x} \sin{\left(J_{1} \right)} - o_{y} \cos{\left(J_{1} \right)} & a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)} & p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

# Right hand of (3.76)
A23456

$\displaystyle \left[\begin{matrix}- \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} & \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} & - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\\\left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \cos{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} & - \left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \sin{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}\\\sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} & - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

$$ \displaystyle A_{1i}T_{6} = A_{23456} \\ $$

より以下の等式群を得る

for idx in range(12):
    display(sym.simplify ( sym.expand( sym.Eq( A1iT6[idx], A23456[idx])) ) )

$\displaystyle n_{x} \cos{\left(J_{1} \right)} + n_{y} \sin{\left(J_{1} \right)} = \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)}$

$\displaystyle o_{x} \cos{\left(J_{1} \right)} + o_{y} \sin{\left(J_{1} \right)} = \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)}$

$\displaystyle a_{x} \cos{\left(J_{1} \right)} + a_{y} \sin{\left(J_{1} \right)} = - \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)}$

$\displaystyle - a_{1} + p_{x} \cos{\left(J_{1} \right)} + p_{y} \sin{\left(J_{1} \right)} = - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}$

$\displaystyle n_{z} = - \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)}$

$\displaystyle o_{z} = \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \sin{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}$

$\displaystyle a_{z} = \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)}$

$\displaystyle p_{z} = a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}$

$\displaystyle n_{x} \sin{\left(J_{1} \right)} - n_{y} \cos{\left(J_{1} \right)} = \sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)}$

$\displaystyle o_{x} \sin{\left(J_{1} \right)} - o_{y} \cos{\left(J_{1} \right)} = - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)}$

$\displaystyle \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} = a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)}$

$\displaystyle p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)} = 0$

J1

# このうち位置に関する項のみを抽出すると
for idx in [3,7,11]:
    display(sym.simplify ( sym.expand( sym.Eq( A1iT6[idx], A23456[idx])) ) )

$\displaystyle - a_{1} + p_{x} \cos{\left(J_{1} \right)} + p_{y} \sin{\left(J_{1} \right)} = - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}$

$\displaystyle p_{z} = a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}$

$\displaystyle p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)} = 0$

# の３元連立方程式を得る．　上記第３式は，
sym.Eq(A1iT6[11] , A23456[11])

$\displaystyle p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)} = 0$

p_x = 0 なら直ちに

sol = sym.solve(p_y * C(J_1), J_1)

for i  in range(len(sol)):
    display( sym.simplify(sym.Eq(J_1,  sol[i])))
print( str(len(sol)) +' equation(s) in total')

$\displaystyle J_{1} = \frac{\pi}{2}$

$\displaystyle J_{1} = \frac{3 \pi}{2}$

2 equation(s) in total

# しかし値域(-PI < J1 < PI)より , p_x = 0 なら直ちに
sol = sym.solve(p_y * C(J_1), J_1)

for i  in range(len(sol)):
    if -Pi < sol[0] and sol[i] < Pi:        
        display( sym.simplify(sym.Eq(J_1,  sol[i])), 'where p_x = 0')

$\displaystyle J_{1} = \frac{\pi}{2}$

'where p_x = 0'

p_y = 0 なら直ちに

sol = sym.solve(p_x * S(J_1), J_1)

for i  in range(len(sol)):
    display( sym.simplify(sym.Eq(J_1,  sol[i])))
print( str(len(sol)) +' equation(s) in total')

$\displaystyle J_{1} = 0$

$\displaystyle J_{1} = \pi$

2 equation(s) in total

# であるが，　同様に値域(-PI < J1 < PI)より , p_y = 0 なら直ちに
sol = sym.solve(p_x * S(J_1), J_1)

for i  in range(len(sol)):
    if -Pi < sol[0] and sol[i] < Pi:        
        display( sym.simplify(sym.Eq(J_1,  sol[i])) ,  'where p_y = 0')

$\displaystyle J_{1} = 0$

'where p_y = 0'

p_x != 0 であることが確定したならば，

# 両辺をp_xで割ることができ，
# p_x != 0 ならば 前述の通りC(J1) != 0であることが必然的に求まるためC(J1)で両辺を割っていいため，
eq =sym.simplify((A1iT6[11] - A23456[11])/p_x/C(J_1))
display(eq)

$\displaystyle \tan{\left(J_{1} \right)} - \frac{p_{y}}{p_{x}}$

# この方程式をJ1について解くと，
sol = sym.solve(eq, J_1)
for i  in range(len(sol)):  
        display( sym.simplify(sym.Eq(J_1,  sol[i])) ,  'where p_x != 0')
sol_J1 = sol

$\displaystyle J_{1} = \operatorname{atan}{\left(\frac{p_{y}}{p_{x}} \right)}$

'where p_x != 0'

がJ1に関する解析解である．

J2

# 次に上記第２式に着目すると
sym.Eq(A1iT6[7] , A23456[7])

$\displaystyle p_{z} = a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}$

# であるから,上記第１式に S(J2) を代入すべく C(J2)を求めると
eq =A1iT6[7] - A23456[7]
target = C(J_2)
CJ2 = obs(eq, target)[0]

$\displaystyle \cos{\left(J_{2} \right)} = \frac{- a_{3} \cos{\left(J_{3} \right)} - d_{4} \sin{\left(J_{3} \right)} + p_{z}}{a_{2}}$

1 equation(s) in total

# 従って，S(J2)を求めるためには，C(J2)**2 + S(J2)**2 = 1 であるから標的となる式を再掲すると，
eq = sym.Eq(A1iT6[3] , A23456[3])
display(eq)

$\displaystyle - a_{1} + p_{x} \cos{\left(J_{1} \right)} + p_{y} \sin{\left(J_{1} \right)} = - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}$

# を得る． J1が既知のため上式の左辺をLとおく
sub = A1iT6[3]
L = sym.symbols('L')
eq = eq.subs(sub, L)
display(sym.trigsimp(eq))

$\displaystyle L = - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}$

# S(J2)について整理すると，
target = S(J_2)
SJ2 = obs(eq, target)[0]

$\displaystyle \sin{\left(J_{2} \right)} = \frac{- L - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}}{a_{2}}$

1 equation(s) in total

# ここでS(J2)**2 + C(J2)**2 = 1 より
eq = sym.trigsimp(sym.Eq((SJ2**2+CJ2**2),1))
display(eq)
display(sym.collect(sym.trigsimp(eq.expand()), [C(J_3),S(J_3)]))
# 方程式をJ3について解くと，
target = J_3
sol_J3 = obs(eq, target)

$\displaystyle \frac{\left(L + a_{3} \sin{\left(J_{3} \right)} - d_{4} \cos{\left(J_{3} \right)}\right)^{2}}{a_{2}^{2}} + \frac{\left(- a_{3} \cos{\left(J_{3} \right)} - d_{4} \sin{\left(J_{3} \right)} + p_{z}\right)^{2}}{a_{2}^{2}} = 1$

$\displaystyle \frac{L^{2} + 2 L a_{3} \sin{\left(J_{3} \right)} - 2 L d_{4} \cos{\left(J_{3} \right)} + a_{3}^{2} - 2 a_{3} p_{z} \cos{\left(J_{3} \right)} + d_{4}^{2} - 2 d_{4} p_{z} \sin{\left(J_{3} \right)} + p_{z}^{2}}{a_{2}^{2}} = 1$

$\displaystyle J_{3} = 2 \operatorname{atan}{\left(\frac{- 2 L a_{3} + 2 d_{4} p_{z} + \sqrt{- L^{4} + 2 L^{2} a_{2}^{2} + 2 L^{2} a_{3}^{2} + 2 L^{2} d_{4}^{2} - 2 L^{2} p_{z}^{2} - a_{2}^{4} + 2 a_{2}^{2} a_{3}^{2} + 2 a_{2}^{2} d_{4}^{2} + 2 a_{2}^{2} p_{z}^{2} - a_{3}^{4} - 2 a_{3}^{2} d_{4}^{2} + 2 a_{3}^{2} p_{z}^{2} - d_{4}^{4} + 2 d_{4}^{2} p_{z}^{2} - p_{z}^{4}}}{L^{2} + 2 L d_{4} - a_{2}^{2} + a_{3}^{2} + 2 a_{3} p_{z} + d_{4}^{2} + p_{z}^{2}} \right)}$

$\displaystyle J_{3} = - 2 \operatorname{atan}{\left(\frac{2 L a_{3} - 2 d_{4} p_{z} + \sqrt{- L^{4} + 2 L^{2} a_{2}^{2} + 2 L^{2} a_{3}^{2} + 2 L^{2} d_{4}^{2} - 2 L^{2} p_{z}^{2} - a_{2}^{4} + 2 a_{2}^{2} a_{3}^{2} + 2 a_{2}^{2} d_{4}^{2} + 2 a_{2}^{2} p_{z}^{2} - a_{3}^{4} - 2 a_{3}^{2} d_{4}^{2} + 2 a_{3}^{2} p_{z}^{2} - d_{4}^{4} + 2 d_{4}^{2} p_{z}^{2} - p_{z}^{4}}}{L^{2} + 2 L d_{4} - a_{2}^{2} + a_{3}^{2} + 2 a_{3} p_{z} + d_{4}^{2} + p_{z}^{2}} \right)}$

2 equation(s) in total

# Masahiro Furukawa
# Aug, 21, 2020
#
# refernce : https://qiita.com/JmpM/items/4bea4997aaf406cca3b4

# J3 に対するCソースを得る
code = sym.ccode(sub, assign_to=('L'), standard='C89')
print("// constant L \n"  + code + "\n")

for ii in range(len(sol_J3)):
    code = sym.ccode(sol_J3[ii], assign_to=('J3'), standard='C89')
    print("// Solusion #"+ str(ii) +"\n"  + code + "\n")

// constant L 
L = -a_1 + p_x*cos(J_1) + p_y*sin(J_1);

// Solusion #0
J3 = 2*atan((-2*L*a_3 + 2*d_4*p_z + sqrt(-pow(L, 4) + 2*pow(L, 2)*pow(a_2, 2) + 2*pow(L, 2)*pow(a_3, 2) + 2*pow(L, 2)*pow(d_4, 2) - 2*pow(L, 2)*pow(p_z, 2) - pow(a_2, 4) + 2*pow(a_2, 2)*pow(a_3, 2) + 2*pow(a_2, 2)*pow(d_4, 2) + 2*pow(a_2, 2)*pow(p_z, 2) - pow(a_3, 4) - 2*pow(a_3, 2)*pow(d_4, 2) + 2*pow(a_3, 2)*pow(p_z, 2) - pow(d_4, 4) + 2*pow(d_4, 2)*pow(p_z, 2) - pow(p_z, 4)))/(pow(L, 2) + 2*L*d_4 - pow(a_2, 2) + pow(a_3, 2) + 2*a_3*p_z + pow(d_4, 2) + pow(p_z, 2)));

// Solusion #1
J3 = -2*atan((2*L*a_3 - 2*d_4*p_z + sqrt(-pow(L, 4) + 2*pow(L, 2)*pow(a_2, 2) + 2*pow(L, 2)*pow(a_3, 2) + 2*pow(L, 2)*pow(d_4, 2) - 2*pow(L, 2)*pow(p_z, 2) - pow(a_2, 4) + 2*pow(a_2, 2)*pow(a_3, 2) + 2*pow(a_2, 2)*pow(d_4, 2) + 2*pow(a_2, 2)*pow(p_z, 2) - pow(a_3, 4) - 2*pow(a_3, 2)*pow(d_4, 2) + 2*pow(a_3, 2)*pow(p_z, 2) - pow(d_4, 4) + 2*pow(d_4, 2)*pow(p_z, 2) - pow(p_z, 4)))/(pow(L, 2) + 2*L*d_4 - pow(a_2, 2) + pow(a_3, 2) + 2*a_3*p_z + pow(d_4, 2) + pow(p_z, 2)));

J2

# 次に，J2について解く．C(J2)について，CJ2として得られているからこれらの方程式から以下を得る．
eq = sym.Eq(C(J_2), CJ2)
display(eq)
sol_J2 = obs(eq, J_2)

$\displaystyle \cos{\left(J_{2} \right)} = \frac{- a_{3} \cos{\left(J_{3} \right)} - d_{4} \sin{\left(J_{3} \right)} + p_{z}}{a_{2}}$

$\displaystyle J_{2} = - \operatorname{acos}{\left(- \frac{a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)} - p_{z}}{a_{2}} \right)} + 2 \pi$

$\displaystyle J_{2} = \operatorname{acos}{\left(\frac{- a_{3} \cos{\left(J_{3} \right)} - d_{4} \sin{\left(J_{3} \right)} + p_{z}}{a_{2}} \right)}$

2 equation(s) in total

# J2 に対するCソースを得る
for ii in range(len(sol_J3)):
    code = sym.ccode(sol_J2[ii], assign_to=('J2'), standard='C89')
    print("// Solusion #"+ str(ii) +"\n"  + code + "\n")

// Solusion #0
J2 = -acos(-(a_3*cos(J_3) + d_4*sin(J_3) - p_z)/a_2) + 2*M_PI;

// Solusion #1
J2 = acos((-a_3*cos(J_3) - d_4*sin(J_3) + p_z)/a_2);

以上の導出から J1, J2, J3 が求まった．

J5

A3iA2iA1iT6 = A3i*A2i*A1i*T6
for idx in range(12):
    display(sym.simplify ( sym.expand( sym.Eq( A3iA2iA1iT6[idx], A456[idx])) ) )

$\displaystyle \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} - \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} = - \frac{n_{x} \sin{\left(J_{1} - J_{3} \right)}}{2} + \frac{n_{x} \sin{\left(J_{1} + J_{3} \right)}}{2} + \frac{n_{y} \cos{\left(J_{1} - J_{3} \right)}}{2} - \frac{n_{y} \cos{\left(J_{1} + J_{3} \right)}}{2} - n_{z} \cos{\left(J_{3} \right)}$

$\displaystyle \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} = - \frac{o_{x} \sin{\left(J_{1} - J_{3} \right)}}{2} + \frac{o_{x} \sin{\left(J_{1} + J_{3} \right)}}{2} + \frac{o_{y} \cos{\left(J_{1} - J_{3} \right)}}{2} - \frac{o_{y} \cos{\left(J_{1} + J_{3} \right)}}{2} - o_{z} \cos{\left(J_{3} \right)}$

$\displaystyle \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} = \frac{a_{x} \sin{\left(J_{1} - J_{3} \right)}}{2} - \frac{a_{x} \sin{\left(J_{1} + J_{3} \right)}}{2} - \frac{a_{y} \cos{\left(J_{1} - J_{3} \right)}}{2} + \frac{a_{y} \cos{\left(J_{1} + J_{3} \right)}}{2} + a_{z} \cos{\left(J_{3} \right)}$

$\displaystyle - a_{1} \sin{\left(J_{3} \right)} + a_{2} \cos{\left(J_{2} - J_{3} \right)} + a_{3} + p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} + p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} - p_{z} \cos{\left(J_{3} \right)} = 0$

$\displaystyle n_{x} \sin{\left(J_{1} \right)} - n_{y} \cos{\left(J_{1} \right)} = \sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)}$

$\displaystyle o_{x} \sin{\left(J_{1} \right)} - o_{y} \cos{\left(J_{1} \right)} = - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)}$

$\displaystyle \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} = a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)}$

$\displaystyle p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)} = 0$

$\displaystyle \sin{\left(J_{5} \right)} \cos{\left(J_{6} \right)} = - \frac{n_{x} \cos{\left(J_{1} - J_{3} \right)}}{2} - \frac{n_{x} \cos{\left(J_{1} + J_{3} \right)}}{2} - \frac{n_{y} \sin{\left(J_{1} - J_{3} \right)}}{2} - \frac{n_{y} \sin{\left(J_{1} + J_{3} \right)}}{2} - n_{z} \sin{\left(J_{3} \right)}$

$\displaystyle \sin{\left(J_{5} \right)} \sin{\left(J_{6} \right)} = \frac{o_{x} \cos{\left(J_{1} - J_{3} \right)}}{2} + \frac{o_{x} \cos{\left(J_{1} + J_{3} \right)}}{2} + \frac{o_{y} \sin{\left(J_{1} - J_{3} \right)}}{2} + \frac{o_{y} \sin{\left(J_{1} + J_{3} \right)}}{2} + o_{z} \sin{\left(J_{3} \right)}$

$\displaystyle \cos{\left(J_{5} \right)} = \frac{a_{x} \cos{\left(J_{1} - J_{3} \right)}}{2} + \frac{a_{x} \cos{\left(J_{1} + J_{3} \right)}}{2} + \frac{a_{y} \sin{\left(J_{1} - J_{3} \right)}}{2} + \frac{a_{y} \sin{\left(J_{1} + J_{3} \right)}}{2} + a_{z} \sin{\left(J_{3} \right)}$

$\displaystyle d_{4} = - a_{1} \cos{\left(J_{3} \right)} + a_{2} \sin{\left(J_{2} - J_{3} \right)} + p_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + p_{z} \sin{\left(J_{3} \right)}$

# 上記式の第１１式は，
idx=10
eq = sym.simplify ( sym.Eq( A3iA2iA1iT6[idx], A456[idx]))
display(eq) 

$\displaystyle \cos{\left(J_{5} \right)} = a_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{z} \sin{\left(J_{3} \right)}$

sol_J5 = obs(eq, J_5)

$\displaystyle J_{5} = - \operatorname{acos}{\left(a_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{z} \sin{\left(J_{3} \right)} \right)} + 2 \pi$

$\displaystyle J_{5} = \operatorname{acos}{\left(a_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{z} \sin{\left(J_{3} \right)} \right)}$

2 equation(s) in total

# J5 に対するCソースを得る
for ii in range(len(sol_J5)):
    code = sym.ccode(sol_J5[ii], assign_to=('J5'), standard='C89')
    print("// Solusion #"+ str(ii) +"\n"  + code + "\n")

// Solusion #0
J5 = -acos(a_x*cos(J_1)*cos(J_3) + a_y*sin(J_1)*cos(J_3) + a_z*sin(J_3)) + 2*M_PI;

// Solusion #1
J5 = acos(a_x*cos(J_1)*cos(J_3) + a_y*sin(J_1)*cos(J_3) + a_z*sin(J_3));

J4

# 上記式の第7式は，
idx=6
eq = sym.simplify ( sym.Eq( A3iA2iA1iT6[idx], A456[idx]))
display(eq) 

$\displaystyle \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} = a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)}$

sol_J4 = obs(eq, J_4)

$\displaystyle J_{4} = \pi - \operatorname{asin}{\left(\frac{a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)}}{\sin{\left(J_{5} \right)}} \right)}$

$\displaystyle J_{4} = \operatorname{asin}{\left(\frac{a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)}}{\sin{\left(J_{5} \right)}} \right)}$

2 equation(s) in total

# J4 に対するCソースを得る
for ii in range(len(sol_J4)):
    code = sym.ccode(sol_J4[ii], assign_to=('J4'), standard='C89')
    print("// Solusion #"+ str(ii) +"\n"  + code + "\n")

// Solusion #0
J4 = M_PI - asin((a_x*sin(J_1) - a_y*cos(J_1))/sin(J_5));

// Solusion #1
J4 = asin((a_x*sin(J_1) - a_y*cos(J_1))/sin(J_5));

J6

A5iA4iA3iA2iA1iT6 = A5i*A4i*A3i*A2i*A1i*T6
for idx in range(12):
    display(sym.simplify ( sym.expand( sym.Eq( A5iA4iA3iA2iA1iT6[idx], A6[idx])) ) )

$\displaystyle \cos{\left(J_{6} \right)} = n_{x} \sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} - n_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} - n_{x} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} - n_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} - n_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} - n_{y} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{5} \right)} - n_{z} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + n_{z} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}$

$\displaystyle \sin{\left(J_{6} \right)} = - o_{x} \sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + o_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + o_{x} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + o_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + o_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} + o_{y} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{5} \right)} + o_{z} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} - o_{z} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}$

$\displaystyle - a_{x} \sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + a_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + a_{x} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + a_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} + a_{y} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{5} \right)} + a_{z} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} - a_{z} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} = 0$

$\displaystyle - a_{1} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} - a_{1} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} + a_{2} \sin{\left(J_{5} \right)} \sin{\left(J_{2} - J_{3} \right)} + a_{2} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{2} - J_{3} \right)} + a_{3} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} - d_{4} \sin{\left(J_{5} \right)} - p_{x} \sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + p_{x} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} + p_{y} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{5} \right)} + p_{z} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} - p_{z} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} = 0$

$\displaystyle \sin{\left(J_{6} \right)} = n_{x} \sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + n_{x} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} + n_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - n_{y} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} - n_{z} \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)}$

$\displaystyle \cos{\left(J_{6} \right)} = o_{x} \sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + o_{x} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} + o_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - o_{y} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} - o_{z} \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)}$

$\displaystyle a_{x} \sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + a_{x} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} + a_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - a_{y} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} - a_{z} \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} = 0$

$\displaystyle - a_{1} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} + a_{2} \sin{\left(J_{4} \right)} \cos{\left(J_{2} - J_{3} \right)} + a_{3} \sin{\left(J_{4} \right)} + p_{x} \sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + p_{x} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} + p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - p_{y} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} - p_{z} \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} = 0$

$\displaystyle n_{x} \sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} - n_{x} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + n_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} - n_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + n_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} - n_{y} \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} + n_{z} \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + n_{z} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} = 0$

$\displaystyle o_{x} \sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} - o_{x} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + o_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} - o_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + o_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} - o_{y} \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} + o_{z} \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + o_{z} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} = 0$

$\displaystyle a_{x} \sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} - a_{x} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + a_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} - a_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + a_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} - a_{y} \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} + a_{z} \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + a_{z} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} = 1$

$\displaystyle a_{1} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} - a_{1} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} - a_{2} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{2} - J_{3} \right)} + a_{2} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{5} \right)} - a_{3} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} - d_{4} \cos{\left(J_{5} \right)} + p_{x} \sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} - p_{x} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + p_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} - p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} - p_{y} \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} + p_{z} \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + p_{z} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} = 0$

# 上記式の第6式は，
idx=5
eq = sym.simplify ( sym.Eq( A5iA4iA3iA2iA1iT6 [idx], A6[idx]))
display(eq) 

$\displaystyle \cos{\left(J_{6} \right)} = o_{x} \left(\sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) + o_{y} \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) - o_{z} \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)}$

sol_J6 = obs(eq, J_6)

$\displaystyle J_{6} = - \operatorname{acos}{\left(o_{x} \sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + o_{x} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} + o_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - o_{y} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} - o_{z} \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \right)} + 2 \pi$

$\displaystyle J_{6} = \operatorname{acos}{\left(o_{x} \sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + o_{x} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} + o_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - o_{y} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} - o_{z} \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \right)}$

2 equation(s) in total

# J6 に対するCソースを得る
for ii in range(len(sol_J6)):
    code = sym.ccode(sol_J6[ii], assign_to=('J6'), standard='C89')
    print("// Solusion #"+ str(ii) +"\n"  + code + "\n")

// Solusion #0
J6 = -acos(o_x*sin(J_1)*cos(J_4) + o_x*sin(J_3)*sin(J_4)*cos(J_1) + o_y*sin(J_1)*sin(J_3)*sin(J_4) - o_y*cos(J_1)*cos(J_4) - o_z*sin(J_4)*cos(J_3)) + 2*M_PI;

// Solusion #1
J6 = acos(o_x*sin(J_1)*cos(J_4) + o_x*sin(J_3)*sin(J_4)*cos(J_1) + o_y*sin(J_1)*sin(J_3)*sin(J_4) - o_y*cos(J_1)*cos(J_4) - o_z*sin(J_4)*cos(J_3));

以上から J4, J5, J6を得る．